﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using Microsoft.Xna.Framework;
using System.Collections;

namespace FUSE
{



        /// <summary>
        /// NOTE: please use descriptive names instead of gc use galaxyComponent, or gravMap vs gravityMap
        /// The extra characters will help undertand the function instantly
        /// also please do not commit broken code
        /// please create a contract to your function before committing
        /// please declare all your variables at the top of the class or function
        /// </summary>
        public static class PhysicsEngine
        {
            public static int k_looks_good = 25;

            /// <summary>
            /// Generates a new gravity map
            /// </summary>
            /// <param name="galaxy"></param>
            /// NOTE: Think about making gravityGrid be type GenericForces[,] this was we can have child SlingForces(for slingOrbit) CrashFroces(for CrashOrbit)
            /// NOTE: Needs to be tested
            /// <returns></returns>
            public static GenericForce[,] createGravityGrid2(Map mapToMark)
            {

                //int planetMass;
                //double crashRadius; //not needed here
                //double slingRadius;

                //int centerX;
                //int centerY;

                //double maxY;
                //int maxX;


                int xSize = mapToMark.getXSize();
                int ySize = mapToMark.getYSize();

                GenericForce[,] newGravityMap = null;


                //Create gravity grid
                newGravityMap = new GenericForce[xSize, ySize];

                for (int xC = 0; xC < xSize; xC++)
                {
                    for (int yC = 0; yC < ySize; yC++)
                    {
                        newGravityMap[xC, yC] = new GenericForce();
                    }
                }

                //For every object with mass in the galaxy table create a slingshot gravity area
                //also a crash gravity area
                foreach (DictionaryEntry entry in mapToMark.galaxyComponentsTable)
                {
                    Console.Write("Before GRID Create");
                    Debug.Assert(entry.Key != null);
                    if (entry.Value is MassedComponent)
                    {
                        plotMassedComponent(entry, newGravityMap);
                    }
                    else if (entry.Value is Wormhole)
                    {
                        //NO forces for wormhole
                    }
                    

                }

                return newGravityMap;


            }

            private static void plotMassedComponent(DictionaryEntry entry, GenericForce[,] newGravityMap)
            {
                double outerRadius;
                double innerRadius;

                //int planetMass;
                
                //Console.Write("IN GRID Create\n\n\n");
                MassedComponent massedComponent = (MassedComponent)entry.Value;

                if (massedComponent is Planet)
                {
                    //cast massedC into planet
                    Planet planet = (Planet) massedComponent;
                    //plot sling forces outer ring
                    innerRadius = planet.OrbitCircle.radius;
                    outerRadius = planet.SlingCircle.radius;
                    plotOrbit(planet, innerRadius, outerRadius, newGravityMap, "slingForce");

                    //plot orbit force inner
                    innerRadius = planet.CrashCircle.radius;
                    outerRadius = planet.OrbitCircle.radius;
                    plotOrbit(planet, innerRadius, outerRadius, newGravityMap, "orbitForce");
                }
                else if (massedComponent is Sun)
                {//Sun has no orbit
                    Sun sun = (Sun) massedComponent;
                    //plot sling forces outer ring
                    innerRadius = sun.CrashCircle.radius;
                    outerRadius = sun.SlingCircle.radius;
                    plotOrbit(sun, innerRadius, outerRadius, newGravityMap, "slingForce");
                }
                else if (massedComponent is Blackhole)
                {
                    Blackhole blackhole = (Blackhole)massedComponent;
                    //plot sling forces outer ring
                    innerRadius = blackhole.CrashCircle.radius;
                    outerRadius = blackhole.SlingCircle.radius;
                    plotOrbit(blackhole, innerRadius, outerRadius, newGravityMap, "slingForce");
                }



            }

            public static void collisionShipPlanet()
            {
                //occurs if ship actually collides with planet (i think?), happens in galaxy: moveShip()
                
                //boom
                
            }

            private static void plotOrbit(MassedComponent massedComponent, double innerRadius, double outerRadius, GenericForce[,] newGravityMap, String typeOfForce)
            {
                int maxX;
                double maxY;
                int centerX;
                int centerY;
                maxY = (int)outerRadius;
                maxX = (int)outerRadius;

                //X and Y counters we will be using to mark inside of orbit
                int xCounter = maxX;
                int yCounter = 0;



                //Find slingshot orbit center(x,y) of this massed component 
                centerX = (int)massedComponent.SlingCircle.centerPOS.X;
                centerY = (int)massedComponent.SlingCircle.centerPOS.Y;

                //Only calculate 90% and reflect it for other 3 quadrents ie 270%
                while (xCounter >= 0 && yCounter <= maxY) //start at the right when x=maxX and y = 0;
                {
                    //if the product of the x and y coordinates are less than the radius
                    if (Math.Sqrt(xCounter * xCounter + yCounter * yCounter) <= outerRadius)
                    {
                        //mark all squares from the center up to x
                        //NOTE: only Squares outside the crash radius needs to be marked, so we can detect its in either crash or sling orbits
                        /**
                                 *    ***
                                 *    ******
                                 *    ********
                                 *  ()********
                                 */
                        for (int x = 0; x <= xCounter; x++)
                        {

                                markGravityMapGrid(x, yCounter, massedComponent, newGravityMap, typeOfForce); //Mark up to x only

                        }
                        //move up one y do it again
                        //Console.Out.WriteLine("Incremented Y " + yCounter);
                        yCounter++;
                    }
                    else                             //move to the left 1 x try again
                    {
                        //System.Console.Out.WriteLine("MarkGrid MOVING TO NEXT LINE x_ " + xCounter);
                        xCounter--;
                    }


                }
            }

            /// <summary>
            /// Mark the global forces on the Gravity map grid, by reflecting for each quadrant
            /// </summary>
            /// <param name="xRelative"></param> x position from the origin being centre
            /// <param name="yRelative"></param> y position from the origin being centre
            /// <param name="centerX"></param> x center of the massed object
            /// <param name="centerY"></param> y center of the massed object
            private static void markGravityMapGrid(int xRelative, int yRelative, MassedComponent massedObject, GenericForce[,] gravityMap, String typeOfForce)
            {

                GenericForce gravity;

                
                int xGlobal;
                int yGlobal;
                int quadrant;
                    
                    //TODO: Check and sum forces
                    //Bottom right quadrant
                    //Find the x and y global to the grid
                    xGlobal = xRelative + (int)massedObject.SlingCircle.centerPOS.X;
                    yGlobal = yRelative + (int)massedObject.SlingCircle.centerPOS.Y;
                    quadrant = 4;


                     GenericForce newGravityForce = new GenericForce();
                    //TODO: Check if xGlobal and yGlobal are out of bounds
                    //Console.WriteLine("Quadrant " + quadrant + " x,y = [" + xGlobal + "," + yGlobal + "]");
                    if ((xGlobal >= 0 ) && (yGlobal >= 0) && (xGlobal < gravityMap.GetLength(0)) && (yGlobal < gravityMap.GetLength(1)))
                    {
                    
                    newGravityForce = calculateGravityForces(xRelative, yRelative, massedObject.Mass, quadrant, typeOfForce);
                        //Add the old vectors to the new vectors
                        newGravityForce.addVectors(gravityMap[xGlobal, yGlobal].Force);
                        gravityMap[xGlobal, yGlobal] = newGravityForce;
                    }
  
                    /*if((gravity.Force.X < 0)  || (gravity.Force.Y < 0) ){
                        Console.WriteLine("["+gravity.Force.X + "," +gravity.Force.Y+"]");
                    }*/
                 

                    //Bottom left quadrant(Bottom left in pic)
                    //Find the x and y global to the grid

                    xGlobal = -xRelative + (int)massedObject.SlingCircle.centerPOS.X;
                    yGlobal = yRelative + (int)massedObject.SlingCircle.centerPOS.Y;
                    quadrant = 3;

                    //Console.WriteLine("Quadrant " + quadrant + " x,y = [" + xGlobal + "," + yGlobal + "]");
                    if ((xGlobal >= 0) && (yGlobal >= 0) && (xGlobal < gravityMap.GetLength(0)) && (yGlobal < gravityMap.GetLength(1)))
                    {
                        newGravityForce = calculateGravityForces(xRelative, yRelative, massedObject.Mass, quadrant, typeOfForce);
                        //Add the old vectors to the new vectors
                        newGravityForce.addVectors(gravityMap[xGlobal, yGlobal].Force);
                        gravityMap[xGlobal, yGlobal] = newGravityForce;
                    }

                    //top right quadrant
                    //Find the x and y global to the grid
                    xGlobal = xRelative + (int)massedObject.SlingCircle.centerPOS.X;
                    yGlobal = -yRelative + (int)massedObject.SlingCircle.centerPOS.Y;
                    quadrant = 1;

                    //Console.WriteLine("Quadrant " + quadrant + " x,y = [" + xGlobal + "," + yGlobal + "]");
                    if ((xGlobal >= 0) && (yGlobal >= 0) && (xGlobal < gravityMap.GetLength(0)) && (yGlobal < gravityMap.GetLength(1)))
                    {
                        newGravityForce = calculateGravityForces(xRelative, yRelative, massedObject.Mass, quadrant, typeOfForce);
                        //Add the old vectors to the new vectors
                        newGravityForce.addVectors(gravityMap[xGlobal, yGlobal].Force);
                        gravityMap[xGlobal, yGlobal] = newGravityForce;
                    }

                    //top left quadrant
                    //Find the x and y global to the grid
                    xGlobal = -xRelative + (int)massedObject.SlingCircle.centerPOS.X;
                    yGlobal = -yRelative + (int)massedObject.SlingCircle.centerPOS.Y;
                    quadrant = 2;

                    //Console.WriteLine("Quadrant " + quadrant + " x,y = [" + xGlobal + "," + yGlobal + "]");
                    if ((xGlobal >= 0) && (yGlobal >= 0) && (xGlobal < gravityMap.GetLength(0)) && (yGlobal < gravityMap.GetLength(1)))
                    {
                        newGravityForce = calculateGravityForces(xRelative, yRelative, massedObject.Mass, quadrant, typeOfForce);
                        //Add the old vectors to the new vectors
                        newGravityForce.addVectors(gravityMap[xGlobal, yGlobal].Force);
                        gravityMap[xGlobal, yGlobal] = newGravityForce;
                    }
            }



            /// <summary>
            /// Calculates the gravity forces based on k_looks_good
            /// </summary>
            /// <param name="xCounter"></param> the counters are relative to the origin of the circle
            /// <param name="yCounter"></param>
            /// <param name="mass"></param>
            /// 
            /// <returns> A gravity x and y force vector</returns>
            private static GenericForce calculateGravityForces(int xRelative, int yRelative, double mass, int quadrant, String typeOfForce )
            {
                GenericForce gravity = null;
                if (typeOfForce.Equals("slingForce"))
                {
                    SlingForce slingForce = new SlingForce();
                    //TODO: calculate acceleration based on how close to planet
                    slingForce.Acceleration = 1.1f;
                    gravity = slingForce;
                    //gravity.Acceleration = 1.1f;
                }
                else if (typeOfForce.Equals("orbitForce"))
                {
                    gravity = new OrbitForce();
                }else
                {
                    gravity = new GenericForce();
                }

                float force = (PhysicsEngine.k_looks_good)/ (float)Math.Sqrt(xRelative * xRelative + yRelative * yRelative + 1);

                double theta = Math.Atan2(yRelative, xRelative);
                //test
                double  xForce = force * Math.Cos(theta);
                double  yForce = force * Math.Sin(theta);

                //quadrant 1 -x y
                if (quadrant == 1)
                {
                    xForce = -1*xForce;
                }
                //quadrant 2 x y
                else if (quadrant == 2)
                {
                    
                }
                //quadrant 3 x -y
                else if(quadrant ==3)
                {
                    yForce = -1*yForce;
                }
                //quadrant 4 -x -y
                else if(quadrant ==4)
                {
                    xForce = -1*xForce;
                    yForce = -1*yForce;
                }  
                //Console.Out.WriteLine("["+xForce+","+yForce+"]");
                gravity.Force = new Vector2((float)xForce, (float)yForce);


                return gravity;


            }

            //NOT BEING USED
            /*
            public static Vector2[,] createGravityGrid(Galaxy galaxy)
            {

                double distance;
                int planetMass;
                //double crashRadius; //not needed here
                double slingRadius;

                int leftX;
                int rightX;

                int centerX;
                int centerY;

                double k_looksgood = 7;
                double theta;
                double force;

                double xForce;
                double yForce;

                Vector2 currentVector;

                int xSize = galaxy.getXSize();
                int ySize = galaxy.getYSize();

                Vector2[,] newGravityMap = null;


                //Create gravity grid
                newGravityMap = new Vector2[xSize, ySize];

                //For every object with mass in the galaxy table create a slingshot gravity area
                //also a crash gravity area
                foreach (GalaxyComponent galaxyComponent in galaxy.GalaxyComponentsTable)
                {
                    if (galaxyComponent is MassedComponent)
                    {
                        MassedComponent massedComponent = (MassedComponent)galaxyComponent;

                        //crashRadius = massedComponent.CrashCircle.radius;
                        slingRadius = massedComponent.SlingCircle.radius;

                        //Massed component is planet
                        planetMass = massedComponent.Mass;

                        //Find slingshot orbit center(x,y) of this massed component 
                        centerX = (int)massedComponent.SlingCircle.centerPOS.X;
                        centerY = (int)massedComponent.SlingCircle.centerPOS.Y;

                        //Do we have to go 360% we could go 90% and reflect each coordinate
                        for (int ycounter = (int)slingRadius; ycounter >= -slingRadius; ycounter--) //start at the top of the circle and move down
                        {

                            //e.g. slingRadius= 5, centerX= 20, yCounter=slingRadius
                            //sqrt(25-(400)-(25)) = 20imaginary??
                            rightX = (int)Math.Sqrt(slingRadius * slingRadius - centerX * centerX - ycounter * ycounter);    //get the x range for each new row
                            leftX = -rightX;

                            for (int xcounter = leftX; xcounter <= rightX; xcounter++)  //start at the left of the row and move right
                            {
                                distance = Math.Sqrt((ycounter - centerY) * (ycounter - centerY) + (xcounter - centerX) * (xcounter - centerX));

                                force = k_looksgood * planetMass / (distance * distance);
                                theta = Math.Atan2(ycounter - centerY, xcounter - centerX);

                                xForce = force * Math.Sin(theta);
                                yForce = force * Math.Cos(theta);
                                currentVector.X = newGravityMap[xcounter, ycounter].X + (float)xForce;
                                currentVector.Y = newGravityMap[xcounter, ycounter].Y + (float)yForce;

                                newGravityMap[xcounter, ycounter] = currentVector;

                            }

                        }

                    }

                }

                return newGravityMap;

            }
             */

            public static String ToStringGalaxy(Map galaxy, Boolean verbose)
            {
                String rtString = "";
                GenericForce squareForce;

                int xSize = galaxy.getXSize();
                int ySize = galaxy.getYSize();
                int x = 0;

                for (int y = 0; y < 200; y++)//xSize
                {

                    for (x = 0; x <200 ; x++)//ySize
                    {


                        squareForce = galaxy.galaxyForces[x, y];

                        if (verbose == true && (squareForce.Force.X != 0 || squareForce.Force.Y != 0))
                        {
                            rtString += "[(" + x + "," + y + ")" + "F: (" + squareForce.Force.X + "," + squareForce.Force.Y + ")]" +
                                        " ";

                        }
                        else if (verbose == false && (squareForce.Force.X != 0 || squareForce.Force.Y != 0))
                        {
                            if ((squareForce.Force.X < 0 || squareForce.Force.Y < 0))
                            {
                                rtString += "-";
                            }
                            else
                            {

                                rtString += "*";
                            }
                        }
                        else
                        {
                            rtString += "0";

                        }
                    }
                    rtString += "\n";
                }

                return rtString;
            }
 
            private static Hashtable createGalaxyComponents()
            {
                throw new NotImplementedException();
            }
            static void main(string[] args)
            {
                
                
            }

            public static LinkedList<Vector2> generateShipOrbitPath(double orbitRadius, Vector2 planetPosition)
            {
                LinkedList<Vector2> orbitPath = new LinkedList<Vector2>();  //make a linked list for the orbit
                double adjustmentRadian = (2*Math.PI)/(4*orbitRadius);      //adjustmentRadian is full circle (2piR)/(number of points to calculate)
                //double adjustmentRadian = (Math.PI / 180);
                double yCounter = orbitRadius;                                 //start at the top of the circle

                //TODO: Need to fix. calculated positions have a distance to center that is greater than radius
                
                Vector2 nextPoint = new Vector2();                          //make a vector to store the value for the next point
                LinkedListNode<Vector2> pointToAdd;
                LinkedListNode<Vector2> previousPoint = new LinkedListNode<Vector2>(Vector2.Zero);
                Boolean down = true;

                for (double currentRadian = 0; currentRadian < 2*Math.PI; currentRadian = currentRadian + adjustmentRadian)
                {
                    nextPoint.X = (float) (int) (planetPosition.X + orbitRadius * Math.Sin(currentRadian));   //use trig to calculate next x point, casted to int to remove decimal created by sin fn
                    nextPoint.Y = (float) (int) (planetPosition.Y + orbitRadius * Math.Cos(currentRadian));   //next y point is counter


                    /*Console.WriteLine("Radians = " + currentRadian);
                    if (Vector2.Distance(nextPoint, planetPosition) > orbitRadius) //TODO: REMOVE COMMENT AFTER FIX
                    {
                        Console.WriteLine("Error. Distance larger than orbit ring. Distance = " 
                            + Vector2.Distance(nextPoint, planetPosition));
                    }*/

                    pointToAdd = new LinkedListNode<Vector2>(nextPoint);

                    if (currentRadian == 0)
                    {
                        orbitPath.AddFirst(pointToAdd);
                    }
                    else
                    {
                        orbitPath.AddAfter(previousPoint, pointToAdd);
                    }

                    previousPoint = pointToAdd;
                    orbitPath.AddLast(nextPoint);       //add new point to list
                }

                
                return orbitPath;
            }


            public static void handleCollision()
            {
                
            }

            public static int findQuadrant(Vector2 movingPosition, Vector2 staticPosition)
            {
                double xTester = (int)movingPosition.X - staticPosition.X;
                double yTester = (int)movingPosition.Y - staticPosition.Y;

                if (xTester > 0 && yTester <= 0)
                    return 1;
                else if (xTester <= 0 && yTester < 0)
                    return 2;
                else if (xTester < 0 && yTester >= 0)
                    return 3;
                else if (xTester >= 0 && yTester > 0)
                    return 4;
                else
                    return 0;
            }

            public static double calculateRadianAngle(double theta, int quadrant)
            {
                double radianAngle = -1;

                if (quadrant == 1)
                    radianAngle = theta;
                else if (quadrant == 2)
                    radianAngle = Math.PI - theta;
                else if (quadrant == 3)
                    radianAngle = theta + Math.PI;
                else if (quadrant == 4)
                    radianAngle = (2 * Math.PI) - theta;

                return radianAngle;
            }
    }

}